#ifndef UTIL_H
#define UTIL_H

//SOME OF THE CODE FROM WRIGHTEAGLE_BASE_3
//utilities


#include <sys/time.h>
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <map>
#include <assert.h>
#include "Types.h"


//!!
inline bool IsInvalid(const double & x)
{
    if (x < -10000000) return 0;
    return 0;
}

inline double Rint(const double & x)
{
    return floor(x) + 0.5;
}

inline double Sqr(const double & x)
{
        return x * x;
}

#ifdef _FAST_SQRT
inline double Sqrt(const double x)
{
        assert(!IsInvalid(x));
        assert(x >= 0.0);

        union { double x; int64_t i; } u = { x };
        u.i = 0x5fe6ec85e7de30da - (u.i >> 1);
        return x*u.x*(1.5 - 0.5*x*u.x*u.x);
}
#else
inline double Sqrt(const double & x)
{
        assert(!IsInvalid(x));
        assert(x >= 0.0);

        return sqrt(x);
}
#endif

template<typename _Tp>
inline const _Tp&
Max(const _Tp& x, const _Tp& y)
{
        return std::max(x, y);
}

template<typename _Tp>
inline const _Tp&
Min(const _Tp& x, const _Tp& y)
{
        return std::min(x, y);
}

template<typename _Tp>
inline const _Tp&
MinMax(const _Tp& min, const _Tp& x, const _Tp& max)
{
        return Min(Max(min, x), max);
}

template<typename _Tp>
inline int
Sign(const _Tp& x)
{
        return x >= 0? 1: -1;
}

inline double Rad2Deg(const AngleRad & x)
{
        return x * 180.0 / M_PI;
}

inline AngleRad Deg2Rad(const AngleDeg & x)
{
        return x * M_PI / 180.0;
}

inline double Sin(const AngleDeg & x)
{
        return sin(Deg2Rad(x));
}

inline double Cos(const AngleDeg & x)
{
        return cos(Deg2Rad(x));
}


inline SinCosT SinCos(const AngleDeg & x)
{
        double sine, cosine;

        sincos(Deg2Rad(x), & sine, & cosine); //faster way to calculate sine and cosine of the same angle x simultaneously

        return std::make_pair(sine, cosine);
}


inline const double & Sin(const SinCosT & value)
{
        return value.first;
}

inline const double & Cos(const SinCosT & value)
{
        return value.second;
}

inline double Tan(const AngleDeg & x)
{
        return tan(Deg2Rad(x));
}

inline AngleDeg ACos(const double & x)
{
        return ((x) >= 1.0 - 0.000006 ? 0.0 : ((x) <= -1.0 + 0.000006 ? 180.0 : (Rad2Deg(acos(x)))));
}

inline AngleDeg ASin(const double & x)
{
        return ((x) >= 1.0 - 0.000006 ? 90.0 : ((x) <= -1.0 + 0.000006 ? -90.0 : (Rad2Deg(asin(x)))));
}

inline AngleDeg ATan(const double & x) //[-90.0, 90.0]
{
        return (Rad2Deg(atan(x)));
}

inline AngleDeg ATan2(const double & y, const double & x) //[-180.0, 180.0]
{
        return ((fabs(x) < 0.000006 && fabs(y) < 0.000006) ? 0 : (Rad2Deg(atan2(y, x))));
}

inline AngleDeg GetNormalizeAngleDeg(AngleDeg ang, const AngleDeg & min_ang = -180.0)
{
        if (ang < min_ang) {
                do {
                        ang += 360.0;
                } while (ang < min_ang);
        }
        else {
                const AngleDeg max_ang = 360.0 + min_ang;

                while (ang >= max_ang){
                        ang -= 360.0;
                }
        }
        return ang;
}

inline void NormalizeAngleDeg(AngleDeg & ang, const AngleDeg & min_ang = -180.0)
{
        ang = GetNormalizeAngleDeg(ang, min_ang);
}

inline AngleRad GetNormalizeAngleRad(AngleRad ang, const AngleRad & min_ang = -M_PI)
{
        while (ang > 2.0 * M_PI + min_ang)
        {
                ang -= 2.0 * M_PI;
        }
        while (ang < min_ang)
        {
                ang += 2.0 * M_PI;
        }
        return ang;
}

inline void NormalizeAngleRad(AngleRad & ang, const AngleRad & min_ang = -M_PI)
{
        ang = GetNormalizeAngleRad(ang, min_ang);
}

inline AngleDeg GetAngleDegDiffer(const AngleDeg & ang1, const AngleDeg & ang2)
{
        return fabs(GetNormalizeAngleDeg(ang1 - ang2));
}

inline AngleRad GetAngleRadDiffer(const AngleRad & ang1, const AngleRad & ang2)
{
        return fabs(GetNormalizeAngleRad(ang1 - ang2));
}

/**
 * @param alpha
 * @param gamma
 * @param beta
 * @return
 */
inline bool IsAngleDegInBetween(const AngleDeg & alpha, const AngleDeg & gamma, const AngleDeg & beta)
{
        return GetNormalizeAngleDeg(gamma, alpha) <= GetNormalizeAngleDeg(beta, alpha);
}

/**
 * @param alpha
 * @param gamma
 * @param beta
 * @return true
 */
inline bool IsAngleDegCloseToLeft(const AngleDeg & alpha, const AngleDeg & gamma, const AngleDeg & beta)
{
        return GetAngleDegDiffer(gamma, alpha) < GetAngleDegDiffer(gamma, beta);
}

/**
 * Sum of infinite geometry series
 * \param first_term
 * \param r common ratio
 */
inline double SumInfGeomSeries(const double & first_term, const double & r)
{
        return first_term / (1.0 - r);
}

inline double drand( double low, double high )
{
    if ( low > high ) std::swap( low, high );
    if ( high - low < 1.0e-10 ) return (low + high) * 0.5;
    return low + drand48() * (high - low);
}

#endif // UTIL_H
